
package boids;

import java.util.ArrayList;

import toxi.geom.Vec2D;

public class Vehicle2D implements SteeringBehaviors
{
	private CollisionBox			collisionBox;

	private Vec2D					position;
	private Vec2D					velocity;
	private Vec2D					accel;

	private VehicleProperties	properties;

	private Vec2D					wanderTarget;
	private Vec2D					circleOffset;
	private Vec2D					circleLocation;
	private float					wanderTheta;

	private int						pathFollowIndex;
	private boolean				pathFollowReverse;
	private boolean				pathFollowDone;

	private float					pathFollowLookahead	= 20.0f;

	public Vehicle2D(Vec2D _pos)
	{
		position = _pos;
		velocity = new Vec2D();
		accel = new Vec2D();

		wanderTheta = (float) (Math.random() * Math.PI * 2.0);
		wanderTarget = new Vec2D();
		circleOffset = new Vec2D();
		circleLocation = new Vec2D();

		pathFollowIndex = 0;
		pathFollowReverse = false;
		pathFollowDone = false;

		setDefaultProperties();
	}

	protected void setDefaultProperties()
	{
		properties = new VehicleProperties();
	}

	public void move()
	{
		velocity.addSelf(accel); // update velocity
		velocity.limit(properties.getMaxSpeed()); // limit speed

		position.addSelf(velocity); // update position
		accel.set(0.0f, 0.0f); // reset accelertion to 0 each cycle

		collisionBox.update(position, velocity);
	}

	public void stop()
	{
		accel.set(0.0f, 0.0f);
	}

	public void wander()
	{
		wanderTheta += (((float) Math.random() * 2 - 1) * properties.getWanderOffset());

		// Now we have to calculate the new location to steer towards on the
		// wander circle
		circleLocation.set(velocity); // Start with velocity
		circleLocation.normalize(); // Normalize to get heading
		if (circleLocation.isZeroVector())
		{
			float theta = (float) Math.random() * 2.0f * (float) Math.PI;
			float x = (float) (Math.cos(theta));
			float y = (float) (Math.sin(theta));
			circleLocation.set(x, y);
		}

		circleLocation.scaleSelf(properties.getWanderDistance()); // Multiply by
		// distance
		circleLocation.addSelf(position); // Make it relative to boid's location

		float x = (float) (Math.cos(wanderTheta));
		float y = (float) (Math.sin(wanderTheta));

		circleOffset.set(x, y);
		circleOffset.scaleSelf(properties.getWanderRadius());
		wanderTarget.set(circleLocation);
		wanderTarget.addSelf(circleOffset);
		accel.addSelf(steer(wanderTarget, false)); // Steer towards
		// it
	}

	public void seek(Vec2D _target)
	{
		accel.addSelf(steer(_target, false));
	}

	public void arrive(Vec2D _target)
	{
		accel.addSelf(steer(_target, true));
	}

	public void avoid(Vec2D _target)
	{
		Vec2D distance = new Vec2D(_target.x, _target.y);
		distance.subSelf(position);

		float d = distance.magnitude();
		if (d < properties.getFleeRadius())
		{
			flee(_target);
		}
		else
		{
			wander();
		}
	}

	public void flee(Vec2D _target)
	{
		Vec2D flee = steer(_target, false);
		flee.scaleSelf(-1.0f);
		accel.addSelf(flee);
	}

	public void avoidObstacles(ArrayList<Obstacle> _obstacles)
	{
		if (_obstacles != null)
		{
			int nearestIndex = -1;
			float minDist = -1.0f;

			for (int i = 0; i < _obstacles.size(); i++)
			{
				float distToKeepBetweenObstacles = _obstacles.get(i).getRadius() + properties.getCollisionRadius();
				float distBetweenPositions = position.sub(_obstacles.get(i).getPosition()).magnitude();

				boolean collideBox = collisionBox.intersectsCircle(_obstacles.get(i).getPosition(), _obstacles.get(i).getRadius());
				boolean collideRad = distBetweenPositions < distToKeepBetweenObstacles;

				if (collideBox || collideRad)
				{
					if (distBetweenPositions < minDist || minDist < 0.0f)
					{
						minDist = distBetweenPositions;
						nearestIndex = i;
					}
				}
			}

			if (nearestIndex >= 0)
			{
				flee(_obstacles.get(nearestIndex).getPosition());
			}
		}
	}

	public void followPath(SplinePath _path, boolean _close, boolean _repeat)
	{
		Vec2D futurePos = new Vec2D(getVelocity().scale(pathFollowLookahead).add(getPosition()));
		Vec2D closestCP = findClosestOnPath(futurePos, _path);

		if (closestCP != null)
		{
			if (closestCP.sub(futurePos).magSquared() > _path.getRadius())
			{
				//seek(getNextToClosestCP(futurePos, closestCP, _path));
				seek(closestCP);
			}
			else
			{
				seek(futurePos);
			}
		}

		// if (_path.getNumPoints() > 0)
		// {
		// Vec2D targetOnPath = _path.getPoint(pathFollowIndex);
		//
		// float distanceToPointOnPath = targetOnPath.sub(position).magSquared();
		// if (distanceToPointOnPath < _path.getRadius() * _path.getRadius())
		// {
		// if (_repeat)
		// {
		// if (!_close)
		// {
		// if (pathFollowIndex == _path.getNumPoints() - 1)
		// {
		// pathFollowReverse = true;
		// }
		// if (pathFollowIndex == 0)
		// {
		// pathFollowReverse = false;
		// }
		//
		// if (pathFollowReverse)
		// {
		// decrementPathFollowIndex(_path);
		// }
		// else
		// {
		// incrementPathFollowIndex(_path);
		// }
		// }
		// else
		// {
		// incrementPathFollowIndex(_path);
		// }
		// }
		// else
		// {
		// if (pathFollowIndex == _path.getNumPoints() - 1)
		// {
		// pathFollowDone = true;
		// }
		//
		// if (!pathFollowDone)
		// {
		// incrementPathFollowIndex(_path);
		// }
		// }
		// }
		//
		// if (pathFollowDone)
		// {
		// arrive(targetOnPath);
		// }
		// else
		// {
		// seek(targetOnPath);
		// }
		// }
	}

	public Vec2D getNextToClosestCP(Vec2D _futurePos, Vec2D _closest, Path _path)
	{
		Vec2D nextToClosest = null;
		for (int i = 0; i < _path.getNumPoints(); i++)
		{
			if (_path.getPoint(i).x == _closest.x && _path.getPoint(i).y == _closest.y)
			{
				if (i > 0 && i < _path.getNumPoints() - 1)
				{
					float distFWD = _path.getPoint(i + 1).sub(_futurePos).magSquared();
					float distBWD = _path.getPoint(i - 1).sub(_futurePos).magSquared();

					if (distFWD < distBWD)
					{
						nextToClosest = _path.getPoint(i + 1);
						System.out.println("next");
					}
					else
					{
						nextToClosest = _path.getPoint(i - 1);
						System.out.println("prev");
					}
				}
				else
					if (i == 0)
					{
						nextToClosest = _path.getStart();
						System.out.println("start");
					}
					else
					{
						nextToClosest = _path.getEnd();
						System.out.println("end");
					}
			}
		}
		
		if (nextToClosest == null)
		{
			nextToClosest = _path.getStart();
			System.out.println("none start");
		}
		
		return nextToClosest;
	}

	public Vec2D findClosestOnPath(Vec2D _pos, SplinePath _path)
	{
		float minDist = -1.0f;
		int minDistIndex = -1;

		for (int i = 0; i < _path.vertices().length; i++)
		{
			Vec2D v = new Vec2D(_path.vertices()[i].x, _path.vertices()[i].y);
			float distSquared = v.sub(_pos).magSquared();
			if (minDist == -1.0f || distSquared < minDist)
			{
				minDist = distSquared;
				minDistIndex = i;
			}
		}

		if (minDistIndex != -1)
		{
			return new Vec2D(_path.vertices()[minDistIndex].x, _path.vertices()[minDistIndex].y	);
		}
		else
		{
			return null;
		}
	}

	public Vec2D steer(Vec2D _target, boolean slowdown)
	{
		Vec2D steer; // the steering vector
		Vec2D desired = new Vec2D(_target);
		desired.subSelf(position); // the desired vector to steer to, pointing
		// from
		// the position to the target
		float d = desired.magnitude(); // distance from the target = magnitude
		// of
		// the desired vector
		// if the distance is greater 0 calculate steering, otherwise return zero
		// vector
		if (d > 0)
		{
			desired.normalize();
			if ((slowdown) && (d < properties.getApproachRadius()))
			{
				desired.scaleSelf(properties.getMaxSpeed() * (d / properties.getApproachRadius()));
			}
			else
			{
				desired.scaleSelf(properties.getMaxSpeed());
			}
			// steering = desired minus velocity
			steer = new Vec2D(desired);
			steer.subSelf(velocity);
			steer.limit(properties.getMaxForce());
		}
		else
		{
			// do not move
			steer = new Vec2D(0, 0);
		}
		return steer;
	}

	public Vec2D getPosition()
	{
		return position;
	}

	public void setPosition(Vec2D _position)
	{
		position = _position;
	}

	public Vec2D getVelocity()
	{
		return velocity;
	}

	public void setVelocity(Vec2D _velocity)
	{
		velocity = _velocity;
	}

	public Vec2D getAccel()
	{
		return accel;
	}

	public void setAccel(Vec2D _accel)
	{
		accel = _accel;
		accel.limit(properties.getMaxForce());
	}

	public Vec2D getWanderTarget()
	{
		return wanderTarget;
	}

	public Vec2D getCircleOffset()
	{
		return circleOffset;
	}

	public Vec2D getCircleLocation()
	{
		return circleLocation;
	}

	public VehicleProperties getProperties()
	{
		return properties;
	}

	public void setProperties(VehicleProperties _properties)
	{
		properties = _properties;
	}

	protected void setDefaultCollisionBox()
	{
		collisionBox = new CollisionBox(position, VehicleProperties.DEFAULT_COLLISION_WIDTH, VehicleProperties.DEFAULT_COLLISION_LOOKAHEAD);
	}

	protected void setCollisionBox(CollisionBox _collisionBox)
	{
		collisionBox = _collisionBox;
	}

	protected CollisionBox getCollisionBox()
	{
		return collisionBox;
	}

	public int getPathFollowIndex()
	{
		return pathFollowIndex;
	}

	public void incrementPathFollowIndex(Path _path)
	{
		pathFollowIndex++;
		pathFollowIndex %= _path.getNumPoints();
	}

	public void decrementPathFollowIndex(Path _path)
	{
		pathFollowIndex--;
		pathFollowIndex %= _path.getNumPoints();
	}

	public void resetPathFollowIndex()
	{
		pathFollowIndex = 0;
		pathFollowDone = false;
	}

	public void setPathFollowIndexToClosestPoint(Path _path)
	{
		float minDist = -1.0f;
		int minDistIndex = -1;

		for (int i = 0; i < _path.getNumPoints(); i++)
		{
			float distSquared = _path.getPoint(i).sub(position).magSquared();
			if (minDist == -1.0f || distSquared < minDist)
			{
				minDist = distSquared;
				minDistIndex = i;
			}
		}

		if (minDistIndex != -1)
		{
			pathFollowIndex = minDistIndex;
		}
		else
		{
			pathFollowIndex = 0;
		}
	}

	/**
	 * @return the pathFollowLookahead
	 */
	public float getPathFollowLookahead()
	{
		return pathFollowLookahead;
	}
}
